/*
  CAN总线数据处理

  处理CAN总线收到的电机数据
*/


#include "can_task.h"

#include "can_use.h"
#include "user_task.h"

#ifdef DEBUG
 CAN_t can;
 CAN_Output_t can_out;
 CAN_RawRx_t can_rx;
 
 
#else 
static CAN_t can;
static CAN_Output_t can_out;
static CAN_RawRx_t can_rx;

#endif


void Task_can(void *argument)
{
   (void)argument;
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;

  /* Device Setup */
  CAN_Init(&can, &(task_runtime.config.chassis_config->can));

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
	
 /* Task Setup */
  while (1) {
#ifdef DEBUG
    task_runtime.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());//osThreadGetStackSpace 是一个函数，用于获取指定线程的剩余堆栈空间
#endif
		
  while (osMessageQueueGet(can.msgq_raw, &can_rx, 0, 0)==osOK) 
		{
      CAN_StoreMsg(&can, &can_rx);
    }
		 //一问一答sick数据指令
	  CAN_Sick_Control(&can);
		
		osMessageQueueReset(task_runtime.msgq.can.feedback.chassisfed3508);
		osMessageQueuePut(task_runtime.msgq.can.feedback.chassisfed3508, &can, 0, 0);
		
		osMessageQueueReset(task_runtime.msgq.can.feedback.pitchfed6020);
		osMessageQueuePut(task_runtime.msgq.can.feedback.pitchfed6020, &can, 0,0);
		
		osMessageQueueReset(task_runtime.msgq.can.feedback.sickfed);
		osMessageQueuePut(task_runtime.msgq.can.feedback.sickfed, &can, 0,0);

        osMessageQueueReset(task_runtime.msgq.can.feedback.chassisfed6020);
		osMessageQueuePut(task_runtime.msgq.can.feedback.chassisfed6020, &can, 0,0);


		if (osMessageQueueGet(task_runtime.msgq.can.output.pitch6020,
		                      &(can_out.pitch6020),0,0) == osOK)  {
			CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);				
	  }									
		if (osMessageQueueGet(task_runtime.msgq.can.output.chassis3508,
                          &(can_out.motor3508), 0, 0) == osOK) {
      CAN_DJIMotor_Control(CAN_MOTOR_3508,&can_out,&can);
    }

		if (osMessageQueueGet(task_runtime.msgq.can.output.chassis6020,
		                      &(can_out.chassis6020),0,0) == osOK)  {
			CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS6020,&can_out,&can);				
	  }	
	
		tick += delay_tick; /* 计算下一个唤醒时刻 */
    osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
      }
}

